package es.uji.viselab.robot;

import java.util.HashSet;
import java.util.Iterator;
import java.util.Set;

import es.uji.viselab.math.Matrix4;
import es.uji.viselab.math.Joints;
import es.uji.viselab.math.PointXYZ;
import es.uji.viselab.math.Pose;
import es.uji.viselab.math.RobotToolLocation;
import es.uji.viselab.robot.links.RobotLink;

public abstract class Robot {

	protected RobotCatalog robotModel;
	protected Matrix4 pose;
	protected double[] joints;
	protected boolean connected = false;

	protected Set<RobotListener> notifiers = new HashSet<RobotListener>();

	public Robot(RobotCatalog rm) {
		robotModel = rm;
	}

	protected void notifyRobotCreated(Robot robot) {
		Iterator<RobotListener> itr = notifiers.iterator();
		while (itr.hasNext()) {
			RobotListener rl = itr.next();
			rl.robotCreated(robot);
		}
	}

	public void addListener(RobotListener rl) {
		notifiers.add(rl);
	}

	public void removeListener(RobotListener rl) {
		notifiers.remove(rl);
	}

	public void robotPositionChanged(Robot robot) {
		notifyPositionChange(robot);
	}

	private void notifyRobotConnected(Robot robot) {
		Iterator<RobotListener> itr = notifiers.iterator();
		while (itr.hasNext()) {
			RobotListener rl = itr.next();
			rl.robotConnected(robot);
		}
	}

	protected void notifyPositionChange(Robot robot) {
		Iterator<RobotListener> itr = notifiers.iterator();

		while (itr.hasNext()) {
			RobotListener rl = itr.next();
			rl.robotPositionChanged(robot);
		}
	}

	public abstract boolean isRobotConnected();

	public void robotDisconnect() {
		notifyRobotDisconnect();
	}

	private void notifyRobotDisconnect() {
		// TODO Auto-generated method stub
	}

	public void connect() throws RobotException {
		notifyRobotConnected(this);
		notifyPositionChange(this);

	}

	public void connect(String ip, int port) throws RobotException {
		notifyRobotConnected(this);
	}

	public abstract boolean isConnected();

	public abstract String ping() throws RobotException;

	public abstract void disconnect() throws RobotException;

	public abstract Joints getJoints() throws RobotException;

	public abstract void move(Joints j) throws RobotException;

	//public abstract PointXYZ increaseMove(int dx, int dy, int dz);

	//public abstract void moveRelative(float x, float y, float z);

	public abstract String getName();

	public abstract boolean isVirtual();

	public Matrix4 getPoseM() {
		   return pose;
	}

	public Pose getPose() {
		return new Pose(pose);
	}

	public abstract void move(double[] velocities) throws RobotException;

	public RobotCatalog getCurrentRobotModel() {
		return robotModel;
	}

	public abstract void setPosition(double[] v) throws RobotException;

	

}
